#include <sheriffos.h>
#include <algorithm/qekf.h>
#include <bsp/peripheral/bmi088.h>

#define ERR() osError("bmi088 err")

namespace bsp {
SpiDevice::SpiDevice(SPI_HandleTypeDef* hspi) : _hspi{hspi} {
}

SpiDevice* SpiDevice::init(SPI_InitTypeDef config) {
    _hspi->Init = config;
    auto err = HAL_SPI_Init(_hspi);
    if (err != HAL_OK) {
        ERR();
    }

    return this;
}

SpiDevice* SpiDevice::receiveDataDMA(uint8_t* buffer, uint16_t len) {
    if ((_hspi == nullptr) || (buffer == nullptr)) {
        ERR();
    }

    auto err = HAL_SPI_Receive_DMA(_hspi, buffer, len);
    if (err != HAL_OK) {
        ERR();
    }
    return this;
}

SpiDevice* SpiDevice::receiveData(uint8_t* buffer, uint16_t len, uint32_t timeout) {
    if ((_hspi == nullptr) || (buffer == nullptr)) {
        ERR();
    }

    auto err = HAL_SPI_Receive(_hspi, buffer, len, timeout);
    if (err != HAL_OK) {
        ERR();
    }
    return this;
}

SpiDevice* SpiDevice::sendDataDMA(uint8_t* buffer, uint16_t len) {
    if ((_hspi == nullptr) || (buffer == nullptr)) {
        ERR();
    }

    auto err = HAL_SPI_Transmit_DMA(_hspi, buffer, len);
    if (err != HAL_OK) {
        ERR();
    }
    return this;
}

SpiDevice* SpiDevice::sendData(uint8_t* buffer, uint16_t len, uint32_t timeout) {
    if ((_hspi == nullptr) || (buffer == nullptr)) {
        ERR();
    }

    auto err = HAL_SPI_Transmit(_hspi, buffer, len, timeout);
    if (err != HAL_OK) {
        ERR();
    }
    return this;
}

SpiDevice* SpiDevice::swapOneByteData(uint8_t tx_data, uint8_t* rx_data_ptr) {
    auto ret = HAL_SPI_TransmitReceive(_hspi, &tx_data, rx_data_ptr, 1, 100);
    if (ret != HAL_OK) {
        ERR();
    }
    return this;
}

SpiDevice* SpiDevice::readMultiReg(uint8_t command, uint8_t* rx_data_ptr, uint8_t length) {
    while (length--) {
        swapOneByteData(command, rx_data_ptr++);
    }
    return this;
}

SpiDevice* SpiDevice::swapDataDMA(uint8_t* tx_data_ptr, uint8_t* rx_data_ptr, uint16_t len) {
    if ((_hspi == nullptr) || (tx_data_ptr == nullptr) || (rx_data_ptr == nullptr)) {
        ERR();
    }

    auto err = HAL_SPI_TransmitReceive_DMA(_hspi, tx_data_ptr, rx_data_ptr, len);
    if (err != HAL_OK) {
        ERR();
    }
    return this;
}

SpiDevice* SpiDevice::swapDataDMA(uint8_t* tx_data_ptr, uint8_t* rx_data_ptr, uint16_t len, uint32_t timeout) {
    if ((_hspi == nullptr) || (tx_data_ptr == nullptr) || (rx_data_ptr == nullptr)) {
        ERR();
    }

    auto err = HAL_SPI_TransmitReceive(_hspi, tx_data_ptr, rx_data_ptr, len, timeout);
    if (err != HAL_OK) {
        ERR();
    }
    return this;
}
} // namespace bsp

namespace bsp {
GpioDriver::GpioDriver(GPIO_TypeDef* gpio, uint16_t pin, GPIO_PinState state) : _gpio{gpio}, _pin{pin}, _state{state} {
}

GpioDriver* GpioDriver::reset() {
    HAL_GPIO_WritePin(_gpio, _pin, GPIO_PIN_RESET);
    read();
    return this;
}

GpioDriver* GpioDriver::set() {
    HAL_GPIO_WritePin(_gpio, _pin, GPIO_PIN_SET);
    read();
    return this;
}

GPIO_PinState GpioDriver::read() {
    _state = HAL_GPIO_ReadPin(_gpio, _pin);
    return _state;
}

GpioDriver* GpioDriver::toggle() {
    HAL_GPIO_TogglePin(_gpio, _pin);
    read();
    return this;
}
} // namespace bsp

extern SPI_HandleTypeDef hspi2;

namespace {
bsp::GpioDriver chip_select_accel{GPIOB, GPIO_PIN_12};
bsp::GpioDriver chip_select_gyro{GPIOB, GPIO_PIN_11};
bsp::SpiDevice bmi088_spi{&hspi2};
os::QuaternionExtendedKalmanFilter qekf_n{10, 0.01F, 10000000, 1, 0.0085F};
Bmi088 gimbal_imu{&bmi088_spi, &chip_select_accel, &chip_select_gyro};
bsp::bmi088::InsDataType gimbal_ins{};

second_t begin;
second_t end;
degree_t yaw_last = 0.0_deg;
degree_t yaw_total_last = 0.0_deg;
degree_t pitch_last = 0.0_deg;
degree_t roll_last = 0.0_deg;
auto* q = qekf_n.xHat[0];
} // namespace

namespace bsp {
namespace bmi088 {
void Start() {
    using namespace units::literals;
    using namespace units::time;
    using namespace units::angle;

    gimbal_imu.init();
    // gimbal_imu.setOffset();
}

void Update() {
    begin = end;
    end = os::getDWTTime_u32();
    gimbal_imu.update();

    // 核心函数,EKF更新四元数
    qekf_n.update(gimbal_imu.gyro_measurements.pitch(), gimbal_imu.gyro_measurements.roll(),
                  gimbal_imu.gyro_measurements.yaw(), gimbal_imu.accel_measurements.x(),
                  gimbal_imu.accel_measurements.y(), gimbal_imu.accel_measurements.z(), (end - begin)());

    // 获取最终数据
    gimbal_ins.pitch =
        -os::math::atan2(2.0F * (q[0] * q[1] + q[2] * q[3]), 2.0F * (q[0] * q[0] + q[3] * q[3]) - 1.0F);
    gimbal_ins.roll = os::math::asin(-2.0F * (q[1] * q[3] - q[0] * q[2]));
    gimbal_ins.yaw =
        -os::math::atan2(2.0F * (q[0] * q[3] + q[1] * q[2]), 2.0F * (q[0] * q[0] + q[1] * q[1]) - 1.0F);

    if (gimbal_ins.yaw - yaw_last > 180.0_deg) {
        gimbal_ins.yaw_round--;
    } else if (gimbal_ins.yaw - yaw_last < -180.0_deg) {
        gimbal_ins.yaw_round++;
    }
    gimbal_ins.yaw_total_angle = 360.0_deg * gimbal_ins.yaw_round + gimbal_ins.yaw;

    gimbal_ins.pitch_speed = (gimbal_ins.pitch - pitch_last) / (end - begin);
    gimbal_ins.roll_speed = (gimbal_ins.roll - roll_last) / (end - begin);
    gimbal_ins.yaw_speed = (gimbal_ins.yaw_total_angle - yaw_total_last) / (end - begin);
    pitch_last = gimbal_ins.pitch;
    roll_last = gimbal_ins.roll;
    yaw_last = gimbal_ins.yaw;
    yaw_total_last = gimbal_ins.yaw_total_angle;
}

InsDataType const& GetImuDataRef() { return gimbal_ins; }
} // namespace bmi088
} // namespace bsp